﻿#region Using Statements
using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using starLiGHT;

#endregion

#region COPYRIGHT

/*
    Copyright (c) 2008, 2009, 2010 
        Roland Rosenkranz (Glatzemann@email.de)
*/

#endregion

#region LICENSE

/*
    This file is part of starLiGHT.Collision.

    starLiGHT.Collision is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    starLiGHT.Collision is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with starLiGHT.Collision.  If not, see <http://www.gnu.org/licenses/>.

    
    ADDITIONAL (commercial) LICENSES for starLiGHT.Collision are available on request.
*/

#endregion

#region Box2D Copyright
/*
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#endregion

#region Version Stuff
// **************[ starLiGHT.Engine SVN ]**********************
// * $Rev:: 1486                  $:  Revision of last commit *
// * $Author:: glatzemann         $:  Author of last commit   *
// * $Date:: 2010-10-08 15:55:47 #$:  Date of last commit     *
// ************************************************************

// **************[ Box2D-Repository Info ]*********************
// Header-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Dynamics/Joints/b2LineJoint.h
// Revision   : r142
// Change-Date: 2011-02-03
//
// Source-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Dynamics/Joints/b2LineJoint.cpp
// Revision   : r145
// Change-Date: 2011-02-07
//
// Status     : Finished!
#endregion

namespace starLiGHT.Physic.TwoD
{
    /// <summary>
    /// Line joint definition. This requires defining a line of 
    /// motion using an axis and an anchor point. The definition uses local 
    /// anchor points and a local axis so that the initial configuration 
    /// can violate the constraint slightly. The joint translation is zero 
    /// when the local anchor points coincide in world space. Using local 
    /// anchors and a local axis helps when saving and loading a game. 
    /// </summary>
    public class LineJointDef : JointDef
    {
	    public LineJointDef()
	    {
		    type = JointType.LINE;
            //localAnchor1 = Vector2.Zero;
            //localAnchor2 = Vector2.Zero;
		    localAxis1 = new Vector2(1.0f, 0.0f);
		    enableMotor = false;
		    maxMotorTorque = 0.0f;
		    motorSpeed = 0.0f;
            frequencyHz = 2.0f;
            dampingRatio = 0.7f;
	    }

        /// <summary>Initialize the bodies, anchors, axis, and reference angle using the world
	    /// anchor and world axis.</summary>
	    public void Initialize(Body bA, Body bB, Vector2 anchor, Vector2 axis)
        {
            body1 = bA;
            body2 = bB;
            localAnchor1 = body1.GetLocalPoint(anchor);
            localAnchor2 = body2.GetLocalPoint(anchor);
            localAxis1 = body1.GetLocalVector(axis);
        }

        /// <summary>The local anchor point relative to body1's origin.</summary>
	    public Vector2 localAnchor1;

        /// <summary>The local anchor point relative to body2's origin.</summary>
	    public Vector2 localAnchor2;

        /// <summary>The local translation axis in body1.</summary>
	    public Vector2 localAxis1;

        /// <summary>Enable/disable the joint motor.</summary>
	    public bool enableMotor;

        /// <summary>The maximum motor torque, usually in N-m.</summary>
	    public float maxMotorTorque;

        /// <summary>The desired motor speed in radians per second.</summary>
	    public float motorSpeed;

        /// <summary>Suspension frequency, zero indicates no suspension</summary>
        public float frequencyHz;

        /// <summary>Suspension damping ratio, one indicates critical damping</summary>
        public float dampingRatio;
    };

    /// <summary>
    /// A line joint. This joint provides two degree of freedom: translation 
    /// along an axis fixed in body1 and rotation in the plane. You can use a 
    /// joint limit to restrict the range of motion and a joint motor to drive 
    /// the rotation or to model rotational friction.
    /// This joint is designed for vehicle suspensions.
    /// </summary>
    public class LineJoint : Joint
    {
	    public override Vector2 GetAnchorA()
        {
            return m_body1.GetWorldPoint(ref m_localAnchorA);
        }

	    public override Vector2 GetAnchorB()
        {
            return m_body2.GetWorldPoint(ref m_localAnchorB);
        }

	    public override Vector2 GetReactionForce(float inv_dt)
        {
            return inv_dt * (m_impulse * m_ay + m_springImpulse * m_ax);
        }

	    public override float GetReactionTorque(float inv_dt)
        {
            //B2_NOT_USED(inv_dt);
            return 0.0f;
        }

	    /// <summary>Get the current joint translation, usually in meters.</summary>
	    public float GetJointTranslation()
        {
            Body bA = m_body1;
            Body bB = m_body2;

            Vector2 pA = bA.GetWorldPoint(ref m_localAnchorA);
            Vector2 pB = bB.GetWorldPoint(ref m_localAnchorB);
            Vector2 d = pB - pA;
            Vector2 axis = bA.GetWorldVector(m_localXAxisA);

            float translation = Vector2.Dot(d, axis);
            return translation;
        }

	    /// <summary>Get the current joint translation speed, usually in meters per second.</summary>
	    public float GetJointSpeed()
        {
            float wA = m_body1.m_angularVelocity;
            float wB = m_body2.m_angularVelocity;
            return wB - wA;
        }

	    /// <summary>Is the joint motor enabled?</summary>
	    public bool IsMotorEnabled()
        {
            return m_enableMotor;
        }

	    /// <summary>Enable/disable the joint motor.</summary>  
	    public void EnableMotor(bool flag)
        {
            m_body1.Awake = true;
            m_body2.Awake = true;
            m_enableMotor = flag;
        }

	    /// <summary>Set the motor speed, usually in radians per second.</summary>
	    public void SetMotorSpeed(float speed)
        {
            m_body1.Awake = true;
            m_body2.Awake = true;
            m_motorSpeed = speed;
        }

	    /// <summary>Get the motor speed, usually in radians per second.</summary>
	    public float GetMotorSpeed()
        {
            return m_motorSpeed;
        }

        /// <summary>
        /// Set/Get the maximum motor force, usually in N-m.
        /// </summary>
        public float MaxMotorTorque
        {
            get
            {
                return m_maxMotorTorque;
            }
            set
            {
                m_body1.Awake = true;
                m_body2.Awake = true;
                m_maxMotorTorque = value;
            }
        }

	    /// <summary>Get the current motor torque given the inverse time step, usually in N-m.</summary>
	    public float GetMotorTorque(float inv_dt)
        {
            return inv_dt * m_motorImpulse;
        }

        /// <summary>
        /// Set/Get the spring frequency in hertz. Setting the frequency to zero disables the spring.
        /// </summary>
        public float SpringFrequencyHz
        {
            get { return m_frequencyHz; }
            set { m_frequencyHz = value; }
        }

        /// <summary>
        /// Set/Get the spring damping ratio
        /// </summary>
        public float SpringDampingRatio
        {
            get { return m_dampingRatio; }
            set { m_dampingRatio = value; }
        }

	    public LineJoint(LineJointDef def)
            : base(def)
        {
            m_localAnchorA = def.localAnchor1;
            m_localAnchorB = def.localAnchor2;
            m_localXAxisA = def.localAxis1;
            m_localYAxisA = Vector2Extensions.Cross(1.0f, m_localXAxisA);

            m_mass = 0.0f;
            m_impulse = 0.0f;
            m_motorMass = 0.0f;
            m_motorImpulse = 0.0f;
            m_springMass = 0.0f;
            m_springImpulse = 0.0f;

            m_maxMotorTorque = def.maxMotorTorque;
            m_motorSpeed = def.motorSpeed;
            m_enableMotor = def.enableMotor;

            m_frequencyHz = def.frequencyHz;
            m_dampingRatio = def.dampingRatio;

            m_bias = 0.0f;
            m_gamma = 0.0f;

            m_ax = Vector2.Zero;
            m_ay = Vector2.Zero;
        }

	    internal override void InitVelocityConstraints(TimeStep step)
        {
            Body bA = m_body1;
            Body bB = m_body2;

            m_localCenter1 = bA.LocalCenter;
            m_localCenter2 = bB.LocalCenter;

            Transform xfA = bA.Transform;
            Transform xfB = bB.Transform;

            // Compute the effective masses.
            Vector2 rA = Math.Mul(xfA.R, m_localAnchorA - m_localCenter1);
            Vector2 rB = Math.Mul(xfB.R, m_localAnchorB - m_localCenter2);
            Vector2 d = bB.m_sweep.c + rB - bA.m_sweep.c - rA;

            m_invMass1 = bA.m_invMass;
            m_invI1 = bA.m_invI;
            m_invMass2 = bB.m_invMass;
            m_invI2 = bB.m_invI;

            // Point to line constraint
            {
                m_ay = Math.Mul(xfA.R, m_localYAxisA);
                m_sAy = Vector2Extensions.Cross(d + rA, m_ay);
                m_sBy = Vector2Extensions.Cross(rB, m_ay);

                m_mass = m_invMass1 + m_invMass2 + m_invI1 * m_sAy * m_sAy + m_invI2 * m_sBy * m_sBy;

                if (m_mass > 0.0f)
                {
                    m_mass = 1.0f / m_mass;
                }
            }

            // Spring constraint
            m_springMass = 0.0f;
            if (m_frequencyHz > 0.0f)
            {
                m_ax = Math.Mul(xfA.R, m_localXAxisA);
                m_sAx = Vector2Extensions.Cross(d + rA, m_ax);
                m_sBx = Vector2Extensions.Cross(rB, m_ax);

                float invMass = m_invMass1 + m_invMass2 + m_invI1 * m_sAx * m_sAx + m_invI2 * m_sBx * m_sBx;

                if (invMass > 0.0f)
                {
                    m_springMass = 1.0f / invMass;

                    float C = Vector2.Dot(d, m_ax);

                    // Frequency
                    float omega = 2.0f * (float)Math.PI * m_frequencyHz;

                    // Damping coefficient
                    float _d = 2.0f * m_springMass * m_dampingRatio * omega;

                    // Spring stiffness
                    float k = m_springMass * omega * omega;

                    // magic formulas
                    m_gamma = step.dt * (_d + step.dt * k);
                    if (m_gamma > 0.0f)
                    {
                        m_gamma = 1.0f / m_gamma;
                    }

                    m_bias = C * step.dt * k * m_gamma;

                    m_springMass = invMass + m_gamma;
                    if (m_springMass > 0.0f)
                    {
                        m_springMass = 1.0f / m_springMass;
                    }
                }
            }
            else
            {
                m_springImpulse = 0.0f;
                m_springMass = 0.0f;
            }

            // Rotational motor
            if (m_enableMotor)
            {
                m_motorMass = m_invI1 + m_invI2;
                if (m_motorMass > 0.0f)
                {
                    m_motorMass = 1.0f / m_motorMass;
                }
            }
            else
            {
                m_motorMass = 0.0f;
                m_motorImpulse = 0.0f;
            }

            if (step.warmStarting)
            {
                // Account for variable time step.
                m_impulse *= step.dtRatio;
                m_springImpulse *= step.dtRatio;
                m_motorImpulse *= step.dtRatio;

                Vector2 P = m_impulse * m_ay + m_springImpulse * m_ax;
                float LA = m_impulse * m_sAy + m_springImpulse * m_sAx + m_motorImpulse;
                float LB = m_impulse * m_sBy + m_springImpulse * m_sBx + m_motorImpulse;

                bA.m_linearVelocity -= m_invMass1 * P;
                bA.m_angularVelocity -= m_invI1 * LA;

                bB.m_linearVelocity += m_invMass2 * P;
                bB.m_angularVelocity += m_invI2 * LB;
            }
            else
            {
                m_impulse = 0.0f;
                m_springImpulse = 0.0f;
                m_motorImpulse = 0.0f;
            }
        }

        internal override void SolveVelocityConstraints(TimeStep step)
        {
            Body bA = m_body1;
            Body bB = m_body2;

            Vector2 vA = bA.m_linearVelocity;
            float wA = bA.m_angularVelocity;
            Vector2 vB = bB.m_linearVelocity;
            float wB = bB.m_angularVelocity;

            // Solve spring constraint
            {
                float Cdot = Vector2.Dot(m_ax, vB - vA) + m_sBx * wB - m_sAx * wA;
                float impulse = -m_springMass * (Cdot + m_bias + m_gamma * m_springImpulse);
                m_springImpulse += impulse;

                Vector2 P = impulse * m_ax;
                float LA = impulse * m_sAx;
                float LB = impulse * m_sBx;

                vA -= m_invMass1 * P;
                wA -= m_invI1 * LA;

                vB += m_invMass2 * P;
                wB += m_invI2 * LB;
            }

            // Solve rotational motor constraint
            {
                float Cdot = wB - wA - m_motorSpeed;
                float impulse = -m_motorMass * Cdot;

                float oldImpulse = m_motorImpulse;
                float maxImpulse = step.dt * m_maxMotorTorque;
                m_motorImpulse = MathHelper.Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
                impulse = m_motorImpulse - oldImpulse;

                wA -= m_invI1 * impulse;
                wB += m_invI2 * impulse;
            }

            // Solve point to line constraint
            {
                float Cdot = Vector2.Dot(m_ay, vB - vA) + m_sBy * wB - m_sAy * wA;
                float impulse = m_mass * (-Cdot);
                m_impulse += impulse;

                Vector2 P = impulse * m_ay;
                float LA = impulse * m_sAy;
                float LB = impulse * m_sBy;

                vA -= m_invMass1 * P;
                wA -= m_invI1 * LA;

                vB += m_invMass2 * P;
                wB += m_invI2 * LB;
            }

            bA.m_linearVelocity = vA;
            bA.m_angularVelocity = wA;
            bB.m_linearVelocity = vB;
            bB.m_angularVelocity = wB;
        }

        internal override bool SolvePositionConstraints(float baumgarte)
        {
	        //B2_NOT_USED(baumgarte);

	        Body bA = m_body1;
	        Body bB = m_body2;

            Vector2 xA = bA.m_sweep.c;
            float angleA = bA.m_sweep.a;

            Vector2 xB = bB.m_sweep.c;
            float angleB = bB.m_sweep.a;

            Mat22 RA = new Mat22(angleA), RB = new Mat22(angleB);

            Vector2 rA = Math.Mul(RA, m_localAnchorA - m_localCenter1);
            Vector2 rB = Math.Mul(RB, m_localAnchorB - m_localCenter2);
            Vector2 d = xB + rB - xA - rA;

            Vector2 ay = Math.Mul(RA, m_localYAxisA);

            float sAy = Vector2Extensions.Cross(d + rA, ay);
            float sBy = Vector2Extensions.Cross(rB, ay);

            float C = Vector2.Dot(d, ay);

            float k = m_invMass1 + m_invMass2 + m_invI1 * m_sAy * m_sAy + m_invI2 * m_sBy * m_sBy;

            float impulse;
            if (k != 0.0f)
            {
                impulse = -C / k;
            }
            else
            {
                impulse = 0.0f;
            }

            Vector2 P = impulse * ay;
            float LA = impulse * sAy;
            float LB = impulse * sBy;

            xA -= m_invMass1 * P;
            angleA -= m_invI1 * LA;
            xB += m_invMass2 * P;
            angleB += m_invI2 * LB;

            // TODO_ERIN remove need for this.
            bA.m_sweep.c = xA;
            bA.m_sweep.a = angleA;
            bB.m_sweep.c = xB;
            bB.m_sweep.a = angleB;
            bA.SynchronizeTransform();
            bB.SynchronizeTransform();

            return System.Math.Abs(C) <= PhysicSettings.LinearSlop;
        }

        internal Vector2 m_localAnchorA;
        internal Vector2 m_localAnchorB;
        internal Vector2 m_localXAxisA;
        internal Vector2 m_localYAxisA;

        internal Vector2 m_ax, m_ay;
        internal float m_sAx, m_sBx;
        internal float m_sAy, m_sBy;

        internal float m_mass;
        internal float m_impulse;
        internal float m_motorMass;

        internal float m_motorImpulse;
        internal float m_springMass;
        internal float m_springImpulse;

        internal float m_maxMotorTorque;
        internal float m_motorSpeed;
        internal float m_frequencyHz;
        internal float m_dampingRatio;
        internal float m_bias;
        internal float m_gamma;

        internal bool m_enableMotor;
    }
}
